import rclpy  
from rclpy.node import Node  
from nav_msgs.msg import Odometry   
from geometry_msgs.msg import Twist  
from std_msgs.msg import Float32MultiArray
from sensor_msgs.msg import Imu  
import numpy as np  
from std_msgs.msg import Int32MultiArray
from std_msgs.msg import Int32
from rcl_interfaces.msg import SetParametersResult
from ai_msgs.msg import PerceptionTargets
from origincar_msg.msg import Sign  # 根据图片的消息类型
from origincar_msg.msg import Data
from std_msgs.msg import String
import math
import datetime
import time
from pyzbar.pyzbar import decode

from sensor_msgs.msg import Image
import cv2

from threading import Thread
import keyboard
import cv_detection 
import sys
import tty
import termios

import os
from openai import OpenAI
import base64
import json

class InertialNavigationNode(Node):  
    def __init__(self):  
        super().__init__('inertial_navigation_node')  # 初始化节点名称

        self.last_log_time_vel_callback = self.get_clock().now()  # 记录上次日志时间
        self.last_log_time_navagation_0 = self.get_clock().now()
        self.last_log_time_navagation_3 = self.get_clock().now()
        self.task3_info = 0
        self.qrcode_info = 0
        # 用于判断是否允许开始导航，1为开始，0为停止
        self.flag = 1

        #用于设置状态，0为简单导航，1为避障导航，2为二维码导航，3为停车点导航  
        #避障导航也就是添加了简单的避障，2和3是将目标点给替代掉  
        self.state = 0

        #目标点切换（任务一任务三启动标志）
        self.task = 0

        #任务中途切换目标位置
        #self.change = 0


        # 创建IMU数据订阅
        self.subscription_imu_data = self.create_subscription(  
            Imu,  
            '/imu/data_raw',  
            self.imu_callback,  
            3
        )
        # 创建目标位置订阅，主要用于订阅目标位置，这个的y的位置偏差较大
        self.subscription_robotvel = self.create_subscription(
            Data,  
            '/robotvel',
            self.robotvel_callback,
            3
        )
        # 创建一个订阅者，订阅 /sigh4return 话题，消息类型为 Int32,上位机信号
        self.sign4return = self.create_subscription(
            Int32,
            '/sign4return',
            self.sign4return_callback,
            10)     
        self.subscription_cone = self.create_subscription(  
            Int32MultiArray,  
            '/cone',  
            self.cone_callback,  
            3)
        self.subscription_park = self.create_subscription(  
            Int32MultiArray,  
            '/park',  
            self.park_callback,  
            3)        

        # 创建速度命令发布者
        self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10)  


        # 初始化状态变量
        self.angular =  0
        self.velocity = 0 #阿克曼小车的速度只有x方向
        self.position = np.array([0.0, 0.0])  # 当前位置信息

        self.target_position = np.array([0.0, 0.0])  # 目标位置        
        self.target_angular = 0.0  # 目标角度  
        
        #由于识别到停车场和二维码后会前往，所以这里设置了相对位置
        self.relativ_target_position = np.array([0.0, 0.0])  # 相对目标位置
        self.park_relativ_position = np.array([0.0, 0.0,0.0,0.0])
        
        #锥桶的相对坐标在这里，分别是xy,宽度和高度
        self.zhuitong_relativ_position = np.array([0.0, 0.0,0.0,0.0])
        self.safety_zhuitong_height = 120
        self.safety_zhuitong_width = 140
        self.zhuitong_position = np.array([0.0, 0.0])

        #这里使用了加速度计校准
        self.sample_size =100
        self.angular_velocity_sample = []
        self.bias_correction = 0
        self.imu_z = 0
        self.robotvel_last_time = self.imu_last_time = self.get_clock().now()   
    
        #参数设置
        self.declare_parameter ('kp',0.04)
        self.declare_parameter ('stop_distance',0.1) 
        self.declare_parameter ('vx',0.6)
        self.declare_parameter ('vx_3',0.3)
        self.declare_parameter ('safe_distance',0.8) 
        self.declare_parameter ('turn_time_1',1.0)
        self.declare_parameter ('turn_time_2',1.2)
        self.declare_parameter ('turn_v',0.4)
        self.declare_parameter ('turn_w',0.01)
        self.declare_parameter ('turn_error',0)
        self.declare_parameter ('zhuitongyuzhi',280) 
        self.declare_parameter ('radius_cone',0.3)

        self.kp = self.get_parameter("kp").value
        self.stop_distance = self.get_parameter("stop_distance").value
        self.vx = self.get_parameter("vx").value
        self.vx_3 = self.get_parameter("vx_3").value
        self.safe_distance = self.get_parameter("safe_distance").value
        self.turn_time_1 = self.get_parameter("turn_time_1").value
        self.turn_time_2 = self.get_parameter("turn_time_2").value
        self.turn_v = self.get_parameter("turn_v").value
        self.turn_w = self.get_parameter("turn_w").value   
        self.turn_error = self.get_parameter("turn_error").value  
        self.zhuitongyuzhi = self.get_parameter("zhuitongyuzhi").value
        self.radius_cone = self.get_parameter("radius_cone").value

        #参数回调函数
        self.add_on_set_parameters_callback(self.parameters_callback)

    def cone_callback(self, msg):  #image话题的回调函数，使用cv获取物体参数，设置小车模式；接受vlm_flag开启大模型图生文

        if(self.task == 1 or self.task == 3): 

            #获取锥桶的底边中点与宽高的坐标（x,y,w,h)
            self.zhuitong_relativ_position = msg.data
            #这里设置一下状态
            if (self.zhuitong_relativ_position[0] != 0  and self.zhuitong_relativ_position[1] > self.zhuitongyuzhi):
                self.state = 1#避障导航模式
                # self.left_cone = int(self.zhuitong_relativ_position[0] - self.zhuitong_relativ_position[2]) - self.turn_error   #校正左转点
                # self.right_cone = int(self.zhuitong_relativ_position[0] + self.zhuitong_relativ_position[2]) + self.turn_error   #校正右转点
            else :
                self.state = 0 #普通导航模式

    def park_callback(self, msg):

        if(self.task == 3): 
            #获取P点的中点与宽高(x,y,w,h)
            self.park_relativ_position = msg.data
            if(self.park_relativ_position[0] != 0):
                self.state = 3 
            else:
                self.state = 0
           
    def sign4return_callback(self,msg): #sign4return话题的回调函数，在foxglove上显示

    # 创建 Sign 消息实例
        if msg.data == 6:
            self.flag = 1
            self.state = 0
            self.task = 3
            self.task3_info = 1

            # 定位任务三中途目标点
            self.target_position[0] = 0  # 目标x和y坐标
            self.target_position[1] = 0
            self.target_angular =  0   
            self.get_logger().info(f"任务三")
            self.get_logger().info(f"结束遥感")

            self.position[0] = 2.5
            self.position[1] = 2.2
            self.angular = -np.pi/2

            ##这里会重置里程计到(2,2)

        if msg.data == -1:
            self.flag = 1
            self.state = 0
            self.task = 1
            #前往二维码区
            self.target_position[0] = 4.6  # 目标x和y坐标
            self.target_position[1] = 1.7
            
            self.target_angular =  0             
            self.get_logger().info(f"任务一")
            self.get_logger().info(f"重置里程计")

            self.angular = 0
            self.position[0] = 0.68
            self.position[1] = 0.2
            ##这里重置了里程计，将他的位置初始化成（0.5，0.2）
            
        if msg.data == 5:
            self.flag = 0
            self.task = 2
            self.get_logger().info(f"任务二")
            self.get_logger().info(f"C区进行遥测")

        return
        
    def imu_callback(self, msg):  # 回调函数，接收IMU数据，并调用navigation_control进行任务一，任务三的控制

        angular_velocity = np.array([msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z])  
        
        if len(self.angular_velocity_sample) <= self.sample_size:
            self.angular_velocity_sample.append(angular_velocity[2])   
        if len(self.angular_velocity_sample) == self.sample_size:
                # 计算偏移量
            self.bias_correction = np.mean(self.angular_velocity_sample)
            self.get_logger().info('偏移量: {}'.format(self.bias_correction))

        if  len(self.angular_velocity_sample) > self.sample_size: 
             self.imu_z = angular_velocity[2]-self.bias_correction

        '''
            current_time = self.get_clock().now()
            dt = (current_time - self.imu_last_time).nanoseconds * 1e-9  # 转换为秒
            self.imu_last_time = current_time

            self.angular += (angular_velocity[2]-self.bias_correction) * dt  # 更新角度
            #限制角度范围在[-pi, pi]之间
            if self.angular > np.pi:
                self.angular = self.angular - 2*np.pi
            if self.angular < -np.pi:
                self.angular = 2*np.pi + self.angular    
        '''
        
        if self.task == 1 :
            self.navigation_control()
        if self.task == 3 :
            distance = math.sqrt(( self.target_position[1] - self.position[1] )**2 + ( self.target_position[0] - self.position[0] )**2)
            #这里小车常常进不去if，因为小车速度快，拐角小，线速度角速度耦合，所以速度大的时候总是在目标点周围却没有满足if条件，就会转圈圈
            #适当调节self.stop_distance与self.vx
            if distance <= self.stop_distance:
                self.target_position[0] = -0.1  # 这里小车位置不太准，不一定设定成0.0,0.0，可以设置成参数具体调整即可
                self.target_position[1] = -0.1
            self.navigation_control()
            if self.task3_info == 1:
                self.get_logger().info('任务三目标位置位置: x={:.6f}, y={:.6f},state={}'.format(self.target_position[0], self.target_position[1],self.state))
                self.task3_info = 0
            
    def robotvel_callback(self , msg):  #回调函数，接收小车速度，并更新小车位置，角度(当前位置: x={:.6f}, y={:.6f}, 角度={:.6f},状态{},任务{})
        x_speed = msg.x
        y_speed = msg.y
        z_speed = msg.z
        current_time = self.get_clock().now()
        dt = (current_time - self.imu_last_time).nanoseconds * 1e-9  # 转换为秒
        self.imu_last_time = current_time

        
        self.position[0]+=1.13*( x_speed * math.cos(self.angular) ) * dt
        self.position[1]+=1.03*( x_speed * math.sin(self.angular) ) * dt
        

        if (x_speed != 0):
            self.angular = self.imu_z * dt + self.angular
            #限制角度范围在[-pi, pi]之间
            if self.angular > np.pi:
                self.angular = self.angular - 2*np.pi
            if self.angular < -np.pi:
                self.angular = 2*np.pi + self.angular    

        # 计算时间差
        elapsed_time = (current_time - self.last_log_time_vel_callback).nanoseconds * 1e-9  # 转换为秒
        if elapsed_time >= 3.0:  # 控制日志 3 秒输出一次
            self.get_logger().info(
                '当前位置: x={:.6f}, y={:.6f}, 角度={:.6f}, 状态{}, 任务{}'.format(
                    self.position[0], self.position[1], self.angular, self.state, self.task
                )
            )
            self.last_log_time_vel_callback = current_time  # 更新日志时间
        #self.get_logger().info('当前位置: x={:.6f}, y={:.6f}, 角度={:.6f},状态{},任务{}'.format(self.position[0], self.position[1], self.angular,self.state,self.task))

    def navigation_control(self): #总导航控制逻辑
        erro = math.sqrt(( self.target_position[1] - self.position[1] )**2 + ( self.target_position[0] - self.position[0] )**2)
        if ( erro > self.stop_distance):
            if self.flag == 1:
                if self.state == 0 :
                    self.navigation_0(self.target_position[0],self.target_position[1])
                elif self.state == 1 :
                    self.navigation_1()
                elif self.state == 2 :
                    self.navigation_2()
                elif self.state == 3:
                    #self.navigation_0(self.target_position[0],self.target_position[1])
                    self.navigation_3()
            else:
                twist_msg = Twist()
                twist_msg.linear.x = 0.00  # 示例：将 x 坐标复位
                twist_msg.angular.z = 0.00  # 使用 角速度复位
                # 发布 Twist 消息
                self.cmd_pub.publish(twist_msg)     

        elif( erro < self.stop_distance):
            cmd_msg = Twist()  
            cmd_msg.linear.x = 0.0  # 设置线性速度  
            cmd_msg.angular.z = 0.0  # 设置角速度  
            self.cmd_pub.publish(cmd_msg)  # 发布控制指令
    
    def navigation_0(self,target_x,target_y): #state=0 无避障简单惯导
                    
        target_angle = np.arctan2((target_y - self.position[1]) , (target_x - self.position[0]))
        angular =  target_angle - self.angular  # 计算角的方向的插值  

        # 构造控制指令  
        cmd_msg = Twist()  
        '''
        if self.task == 3:
            cmd_msg.linear.x = self.vx_3  # 设置线性速度  
            cmd_msg.angular.z = 1 * angular  # 设置角速度  
        
        if self.task == 1:
        '''
        cmd_msg.linear.x = self.vx
        angular = max( min(angular,0.8) , -0.8)
        cmd_msg.angular.z = 2 * angular  # 设置角速度  
        self.cmd_pub.publish(cmd_msg)  # 发布控制指令  

        # 打印当前的角度和转速（调试用）  
        # 计算时间差
        current_time_navagation_0 = self.get_clock().now()
        elapsed_time = (current_time_navagation_0 - self.last_log_time_navagation_0).nanoseconds * 1e-9  # 转换为秒
        if elapsed_time >= 3.0:  # 控制日志 3 秒输出一次
            self.get_logger().info('目标角度: target_angle={:.6f},'.format(target_angle))
            self.get_logger().info('当前角速度: angular={:.6f},'.format(cmd_msg.angular.z))
            self.last_log_time_navagation_0 = current_time_navagation_0 
  
    def navigation_1(self): #state=1 避障惯导

        self.get_logger().info(f"识别到锥桶")

        #锥桶在右，向左避障
        if(self.zhuitong_relativ_position[0] > 320 and self.zhuitong_relativ_position[0] < 500 ):
            if(self.task == 1):
                    self.get_logger().info(f"任务1向左避障")
                    self.navigation_0(self.position[0]+0.2,self.position[1] + 2.0)
                    #time.sleep(0.05)
            elif(self.task == 3):
                    self.navigation_0(self.position[0]-0.2,self.position[1]-3.0)
                    #self.action_cone()
                    self.get_logger().info(f"任务3向左避障")
                    #time.sleep(0.05)
        #锥桶在左，向右避障
        elif(self.zhuitong_relativ_position[0] < 320 and self.zhuitong_relativ_position[0] >150 ):
            if(self.task == 1):
                     self.navigation_0(self.position[0]+0.2,self.position[1] - 2.0)
                     self.get_logger().info(f"任务1向右避障")
                     #time.sleep(0.05)
            elif(self.task == 3):
                    #self.action_cone()
                    self.navigation_0(self.position[0]-0.2,self.position[1]+ 3.0)   
                    # cmd_msg = Twist()  
                    # cmd_msg.linear.x = self.vx
                    # cmd_msg.angular.z = - 2.0
                    # self.cmd_pub.publish(cmd_msg)  # 发布控制指令  
                    self.get_logger().info(f"任务3向右避障")
                    #time.sleep(0.05)
        else:
            self.navigation_0(self.target_position[0],self.target_position[1])

    def navigation_2(self): #state=2，二维码导航控制 实际上并未使用
        cmd_msg = Twist()  
        cmd_msg.linear.x = self.vx  # 设置线性速度  
        cmd_msg.angular.z = - self.kp * (self.park_relativ_position[0] - 320) # 设置角速度
        self.get_logger().info('前往二维码角速度: angular.z={:.6f},'.format(cmd_msg.angular.z ))
        self.cmd_pub.publish(cmd_msg)  # 发布控制指令

    def navigation_3(self): #state=3，cv识别到停车点，进行导航，
        cmd_msg = Twist()  
        cmd_msg.linear.x = self.vx_3  # 设置线性速度  
        cmd_msg.angular.z = - self.kp * (self.park_relativ_position[0] - 320) # 设置角速度

        self.cmd_pub.publish(cmd_msg)  # 发布控制指令

        current_time_navigation_3 = self.get_clock().now()
        elapsed_time = (current_time_navigation_3 - self.last_log_time_navigation_3).nanoseconds * 1e-9  # 转换为秒
        if elapsed_time >= 3.0:  # 控制日志 3 秒输出一次
            self.get_logger().info('返回P点速度: linear.x={:.6f},'.format(cmd_msg.linear.x ))
            self.get_logger().info('返回P点角速度: angular.z={:.6f},'.format(cmd_msg.angular.z ))
            self.last_log_time_navigation_3 = current_time_navigation_3 

    def action_cone(self): #另一种避障逻辑，并未使用
        twist_msg = Twist()
        if self.zhuitong_relativ_position[0] >= 320:
            if (320-self.left_cone ) > 0:
                twist_msg.angular.z = self.turn_w * (320-self.left_cone)
                if self.task ==1:
                    twist_msg.linear.x = self.vx
                if self.task ==3:
                    twist_msg.linear.x = self.vx_3
                self.get_logger().info(f"{self.left_cone}")
                self.get_logger().info(f"{twist_msg.angular.z}")  
                self.cmd_pub.publish(twist_msg)    
                return                  
            else :
                self.navigation_0(self.target_position[0],self.target_position[1])
                
        if self.zhuitong_relativ_position[0] < 320:
            if (320-self.right_cone ) < 0:
                twist_msg.angular.z = self.turn_w * (320-self.right_cone)
                if self.task ==1:
                    twist_msg.linear.x = self.vx
                if self.task ==3:
                    twist_msg.linear.x = self.vx_3
                self.get_logger().info(f"{self.right_cone}")  
                self.get_logger().info(f"{twist_msg.angular.z}")  
                self.cmd_pub.publish(twist_msg) 
                return
            else :
                self.navigation_0(self.target_position[0],self.target_position[1]) 
    
    def parameters_callback(self,paramters): #参数设置回调函数
        for paramter in paramters:
            self.get_logger().info("Parameter: {} - Value: {}".format(paramter.name, paramter.value))
            if paramter.name == "kp":
                self.kp = paramter.value
            elif paramter.name == "vx":
                self.vx = paramter.value   
            elif paramter.name == "vx_3":
                self.vx_3 = paramter.value   
            elif paramter.name == "stop_distance":
                self.stop_distance = paramter.value  
            elif paramter.name == "safe_distance":
                self.safe_distance = paramter.value
            elif paramter.name == "turn_time_1":
                self.turn_time_1 = paramter.value  
            elif paramter.name == "turn_time_2":
                self.turn_time_2 = paramter.value
            elif paramter.name == "turn_v":
                self.turn_v = paramter.value  
            elif paramter.name == "turn_w":
                self.turn_w = paramter.value 
            elif paramter.name == "turn_error":
                self.error = paramter.value    
            elif paramter.name == "zhuitongyuzhi":
                self.zhuitongyuzhi = paramter.value  
            elif paramter.name == "radius_cone":
                self.radius_cone = paramter.value  
     
        return SetParametersResult(successful=True)

def main(args=None):  
    rclpy.init(args=args)  # 初始化rclpy  
    navigation = InertialNavigationNode()  #
    rclpy.spin(navigation)
    navigation.destroy_node()  # 销毁节点
    rclpy.shutdown()
  
if __name__ == '__main__':  
    main()  # 入口函数

